#pragma once
#include <cmath>
#include "FMatrix3X3.hpp"
#include "vector3.h"
namespace Math
{
    /*
    *  四元数定义
    *  定义一个单位向量n 为旋转轴， 旋转角度为 θ。  则[θ, n]表示轴-角坐标系的角位移
    *  四元素表示 [w,v] = [cos(θ/2), sin(θ/2) * n]  注: v表示一个三维向量，n为单位向量

    */
    class FQuaternions
    {
    public:
        /* [w,x,y,z]*/
        float w;
        float x, y, z;

        FQuaternions() {}
        FQuaternions(float angle, float x, float y, float z)
        {
            // 使用弧度制
            w = angle / 2;
            // normalize vec3
            float total = sqrt(x * x + y * y + z * z);
            this->x = x / total;
            this->y = y / total;
            this->z = z / total;
        }

        // FQuaternions(const FQuaternions &q)
        // 四元素叉乘
        FQuaternions operator*(const FQuaternions &q) const
        {
            //[w1, v1] * [w2, v2] = [ w1w2- v1v2 , w1v2 + w2v1 + v1 x v2]
            FQuaternions result{};
            result.w = w * q.w - (x * q.x + y * q.y + z * q.z);
            result.x = (w * q.x + x * q.w) + (y * q.z - z * q.y);
            result.y = (w * q.y + y * q.w) + (z * q.x - x * q.z);
            result.z = (w * q.z + z * q.w) + (x * q.y - y * q.x);
            return result;
        }

        FQuaternions operator*(float v) const
        {
            FQuaternions result{};
            result.w = v * w;
            result.x = v * x;
            result.y = v * y;
            result.z = v * z;
            return result;
        }

        FQuaternions operator/(float v) const
        {
            FQuaternions result{};
            result.w = w / v;
            result.x = x / v;
            result.y = y / v;
            result.z = z / v;
            return result;
        }

        FQuaternions &operator*=(float v)
        {
            w *= v;
            x *= v;
            y *= v;
            z *= v;
            return *this;
        }

        FQuaternions &operator/=(float v)
        {
            w /= v;
            x /= v;
            y /= v;
            z /= v;
            return *this;
        }

        float length() const
        {
            return sqrt(w * w + x * x + y * y + z * z);
        }

        FQuaternions inverse() const
        {
            FQuaternions conjugate{w, -x, -y, -z};
            float length = this->length();
            return conjugate / length;
        }

        FQuaternions &exponent(float exponent)
        {
            // check for the case of an identity quaternion.
            // this will protect against divide by zero.
            if (fabs(w) < 0.9999f)
            {
                // extract the half angle alpha (alphat = theta/2)
                float alpha = acos(w);

                // compute new alpha value;
                float newAlpha = alpha * exponent;

                w = cos(newAlpha);

                float mult = sin(newAlpha) / sin(alpha);
                x *= mult;
                y *= mult;
                z *= mult;
            }
            return *this;
        }
    };

    float dot(const FQuaternions &q1, const FQuaternions &q2)
    {
        return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
    }

    FQuaternions cross(const FQuaternions &q1, const FQuaternions &q2)
    {
        FQuaternions result{};
        result.w = q1.w * q2.w - (q1.x * q2.x + q1.y * q2.y + q1.z * q2.z);
        result.x = (q1.w * q2.x + q1.x * q2.w) + (q1.y * q2.z - q1.z * q2.y);
        result.y = (q1.w * q2.y + q1.y * q2.w) + (q1.z * q2.x - q1.x * q2.z);
        result.z = (q1.w * q2.z + q1.z * q2.w) + (q1.x * q2.y - q1.y * q2.x);
        return result;
    }

    FQuaternions conjugate(const FQuaternions &q)
    {
        FQuaternions result{q};
        result.x *= -1;
        result.y *= -1;
        result.z *= -1;
        return result;
    }

    /*
     * slerp(q,p,t) = (p*q^-1)^t * q
     */
    FQuaternions slerp(const FQuaternions &q1, const FQuaternions &q2, float t)
    {
        FQuaternions temp(q2);
        FQuaternions result{};

        float cosOmega = dot(q1, temp);
        // 如果点乘为负，则反转一个四元素以获取最短的4d "弧"
        if (cosOmega < 0.0f)
        {
            temp *= -1;
            cosOmega *= -1;
        }

        // 检查是否过于接近 以避免除0
        float k0, k1;
        if (cosOmega > 0.9999f)
        {
            // very close -- just use linear interpolation
            k0 = 1.0f - t;
            k1 = t;
        }
        else
        {
            float sinOmega = sqrt(1.0f - cosOmega * cosOmega);
            float omega = atan2(sinOmega, cosOmega);
            k0 = sin((1.0f - t) * omega) / sinOmega;
            k1 = sin(t * omega) / sinOmega;
        }

        result.w = q1.w * k0 + temp.w * k1;
        result.x = q1.x * k0 + temp.x * k1;
        result.y = q1.y * k0 + temp.y * k1;
        result.z = q1.z * k0 + temp.z * k1;

        return result;
    }

    FMatrix3X3 toMatrix(const FQuaternions &quat)
    {
        // 四元素必须是单位四元素

        FMatrix3X3 result{};
        result.m11 = 1 - 2 * quat.y * quat.y - 2 * quat.z * quat.z;
        result.m12 = 2 * quat.x * quat.y + 2 * quat.w * quat.z;
        result.m13 = 2 * quat.x * quat.z - 2 * quat.w * quat.y;

        result.m21 = 2 * quat.x * quat.y - 2 * quat.w * quat.z;
        result.m22 = 1 - 2 * quat.x * quat.x - 2 * quat.z * quat.z;
        result.m23 = 2 * quat.y * quat.z + 2 * quat.w * quat.x;

        result.m31 = 2 * quat.x * quat.z + 2 * quat.w * quat.y;
        result.m32 = 2 * quat.y * quat.z - 2 * quat.w * quat.x;
        result.m33 = 1 - 2 * quat.x * quat.x - 2 * quat.y * quat.y;
        return result;
    }
    
    FQuaternions toQuat(const FMatrix3X3& mat)
    {

    }
} // namespace Math
